/** 
 * balance.c - The C source file of the task
 * NOTE: This file is based on C99 and rt-thread
 *      该文件实现的任务有：
 *      - 平衡控制
 *      - 移动解算
 *
 * Copyright (c) 2020-, garbage classify Project
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author            Notes           mail
 * 2021-04-02     江榕煜       first version   2373180028@qq.com
**/

#include "system.h"

#define NONE_MOT_LOOP

#define VEL2ANG_PSC 10  // 速度环至角度环控制分频器
#ifndef NONE_MOT_LOOP
#define ANG2MOT_PSC 10  // 角度环到电机环分配器
#endif

#define DISTANCE_CTRL   // 使能距离控制模式
#define VEL_CTRL_V2     // 速度控制模式

/* 输入数据 */
static volatile WT931_9AxisData * IMU = RT_NULL;    // IMU数据
int32_t angle_bias = 660;   // 机械重心偏移，效果非常好！
int32_t ANGLE_threshold_VEL = 2000;
int32_t ANGLE_threshold_VEL_FACTOR = 2;
int32_t ANG_MIN = -7000,ANG_MAX = 7000;
extern int32_t wheelSpeed_Left, wheelSpeed_Right;   // 左右轮速度

#ifdef DISTANCE_CTRL
double distance =0;
#endif

/* 输出量 */
volatile double motorDuty_Left,motorDuty_Right;
volatile uint32_t FLAG_TOO_SLANT = 0;  // 过于倾倒标志位，当为1时，关闭速度环，优先保障角度 

ALIGN(RT_ALIGN_SIZE)
static struct rt_thread thread_balance;
static char stack_balance[2048];

volatile uint32_t TRIG_BALANCE_ENABLE = 0;

// 两轮子补偿量
double Motor1_offset = 0;
double Motor2_offset = 0;


// PI 速度环
PID PID_vel;
volatile double vel_DeltaI =0;

// PD 角度环参数
PID PID_ang;

// PI 旋转环
PID PID_rotate;

#ifndef NONE_MOT_LOOP
// PID 电机速度环
PID PID_motorLeft;
PID PID_motorRight;
#endif

double Kp_Ang = 0.01;

/*-------------------函数原型声明--------------*/

/** @brief  PID计算
 *  @note   增量
 *  @author 江榕煜（2021.6.6）
 *  @param  
 *      _PIDer(PID *)   pid数据模块
 *      input(double)   输入
 *  @retval
 *      output(double)  输出
**/
static double PID_calculate(PID* _PIDer,double input);

#ifdef DISTANCE_CTRL
/** @brief  PI计算
 *  @note   位置式
 *  @author 江榕煜（2021.6.11）
 *  @param  
 *      _PIDer(PID *)   pid数据模块
 *      input(double)   输入
 *      integralComp(double)    积分补偿量
 *  @retval
 *      output(double)  输出
**/
static double PI_calculate(PID* _PIDer,double input,double integralComp);
#endif

/** @brief  PD计算
 *  @note   位置式
 *          采用外部输入微分的形式
 *  @author 江榕煜（2021.6.6）
 *  @param  
 *      _PIDer(PID *)   pid数据模块
 *      input(double)   输入
 *      inputD(double)  输入的微分量
 *  @retval
 *      output(double)  输出
**/
static double PD_calculate(PID* _PIDer,double input,double inputD);

/** @brief  转向控制环
 *  @note   使用PI控制
 *  @author 江榕煜（2021.6.12）
 *  @param  
 *          _PIDer(PID*)            控制器
 *          rotateSpeed(double)     期望旋转速度
 *          rotateDetect(double)    检测到的旋转速度
 *  @retval
 *          output  输出
**/
static double turn_calculate(PID* _PIDer, double rotateSpeed, double rotateDetect);

/*----------------------具体实现---------------------*/

/** @brief  平衡控制线程
 *  @note   200Hz
 *  @author 江榕煜（2021.4.2）
 *  @param 
 *  @retval
**/
static void thread_balance_Entry(void* entryParam)
{
    uint32_t psc_vel2Ang =0,psc_ang2Mot = 0;    // 分频器
    double cache;
    while(1)
    {
        if(TRIG_BALANCE_ENABLE) // 使能控制
        {
            
            #ifndef NONE_MOT_LOOP
            if(psc_ang2Mot >= ANG2MOT_PSC)
            {
                psc_ang2Mot = 0;    // 分频器清零
                psc_vel2Ang++;
                /* 角度环 */
                PID_motorLeft.target = PID_calculate(&PID_ang,IMU->Angle.Angle[0] - angle_bias);
                PID_motorRight.target = PID_motorLeft.target;
            }
            
            psc_ang2Mot++;
            
            /* 电机速度环 */
            // 注意：速度采样为20Hz！！！
            motorDuty_Left = PID_calculate(&PID_motorLeft,wheelSpeed_Left);
            motorDuty_Right = PID_calculate(&PID_motorRight,wheelSpeed_Right);
            
            #else   // 不用电机速度环
            
            /* 角度直立环 */
            motorDuty_Left = PD_calculate(&PID_ang,(double)IMU->Angle.Angle[0]-angle_bias,IMU->Gyro.w[0]);
            
            /* 速度环 */
            #define VEL_CTRL_DEBUG_V2
            #ifndef VEL_CTRL_DEBUG_V1
            if((IMU->Angle.Angle[0]-angle_bias) < ANGLE_threshold_VEL 
                && (IMU->Angle.Angle[0]-angle_bias) > (-ANGLE_threshold_VEL)
                //&& ((double)wheelSpeed_Left+(double)wheelSpeed_Right)/2 < 1000
                    )
            {   // 在阈值内，可以进行速度控制
                FLAG_TOO_SLANT = 0;
            }    
            else if((IMU->Angle.Angle[0]-angle_bias) > (ANGLE_threshold_VEL*ANGLE_threshold_VEL_FACTOR)
                      ||
                    (IMU->Angle.Angle[0]-angle_bias) < (-ANGLE_threshold_VEL*ANGLE_threshold_VEL_FACTOR))
            {   // 严重超出阈值，停止速度控制
                FLAG_TOO_SLANT = 1;
                PID_vel.SumI = 0;   PID_vel.output = 0;
                
            }
            if(!FLAG_TOO_SLANT) // 只有标志位标识当前平衡时才进行速度控制
            #else
            if((IMU->Angle.Angle[0]-angle_bias) < ANGLE_threshold_VEL && (IMU->Angle.Angle[0]-angle_bias) > (-ANGLE_threshold_VEL) )
            #endif
            {
                motorDuty_Left += 
                #ifndef DISTANCE_CTRL
                                PID_calculate(&PID_vel,((double)wheelSpeed_Left+(double)wheelSpeed_Right)/2);
                #else
                
                                PI_calculate(&PID_vel,
                                             ((double)wheelSpeed_Left+(double)wheelSpeed_Right)/2,
                                             vel_DeltaI);
                #endif
            }
            else{
                PID_vel.output = 0;
                PID_vel.SumI = 0;
            }
            motorDuty_Right = motorDuty_Left;
            
            /* 旋转PI环 */
            cache = PI_calculate(&PID_rotate,(double)(IMU->Gyro.w[2]),0);
            motorDuty_Left -= cache;
            motorDuty_Right += cache;
            #endif
            
            /* 防倾倒保护 */
            /* 提起等情况，根据爆速度判断 */
            if((IMU->Angle.Angle[0]-angle_bias) > ANG_MAX || // 倒下判断
                (IMU->Angle.Angle[0]-angle_bias) < ANG_MIN ||
                wheelSpeed_Left <-5000 || wheelSpeed_Left > 5000 ||
                wheelSpeed_Right < -5000 || wheelSpeed_Right > 5000)
            {
                motorDuty_Left = 0; // 关停电机
                motorDuty_Right = 0;
                TRIG_BALANCE_ENABLE = 0;
                PID_vel.SumI = 0;   PID_vel.output = 0;
                PID_ang.output = 0;
                rt_kprintf("异常！停止控制！\n");
            }
         }         
        
        /* 设置电机转速 */
        DCMotor_SetSpeed(0,motorDuty_Left);
        DCMotor_SetSpeed(1,motorDuty_Right);
            
        /* 挂起 */
        rt_thread_mdelay(2);    // 500Hz
    }
}

/** @brief  使能平衡控制接口
 *  @note   
 *  @author 江榕煜（2021.6.15）
 *  @param  None
 *  @retval None
**/
void ENABLE_BALANCE_CTRL(void)
{
    PID_vel.SumI = 0;
    PID_vel.output  =0;
    vel_DeltaI = 0;
    PID_rotate.target = 0;
    
    TRIG_BALANCE_ENABLE = 1;
}

/** @brief  失能平衡控制接口
 *  @note   
 *  @author 江榕煜（2021.6.15）
 *  @param  None
 *  @retval None
**/
void DISABLE_BALANCE_CTRL(void)
{
    TRIG_BALANCE_ENABLE = 0;
    motorDuty_Left = 0; // 关停电机
    motorDuty_Right = 0;
    PID_vel.SumI = 0;   PID_vel.output = 0;
    PID_ang.output = 0;
    
    /* 设置电机转速 */
    DCMotor_SetSpeed(0,0);
    DCMotor_SetSpeed(1,0);
    
}

/** @brief  数据采样线程初始化
 *  @note   
 *  @author 江榕煜（2021.4.2）
 *  @param 
 *          imuPtr(void *)  IMU数据对象
 *  @retval
 *          None
**/
void thread_balance_Init(void* imuPtr)
{
    /* PID控制初始化 */
    #ifndef NONE_MOT_LOOP
    PID_motorLeft.Kp = 0.6; PID_motorRight.Kp = 0.6;
    PID_motorLeft.Ki = 0.045; PID_motorRight.Ki = 0.045;
    PID_motorLeft.Kd = 0.2; PID_motorRight.Kd = 0.2;
    PID_motorLeft.OUT_MAX = 100; PID_motorRight.OUT_MAX = 100;
    PID_motorLeft.OUT_MIN = -100; PID_motorRight.OUT_MIN = -100;
    PID_motorLeft.err_Pre = 0; PID_motorRight.err_Pre = 0;
    PID_motorLeft.err_PPre = 0; PID_motorRight.err_PPre = 0;
    PID_motorLeft.target = 0; PID_motorRight.target = 0;
    #endif
    
    // 角度环，注：参数已校准死
    PID_ang.Kp = 0.3;
    PID_ang.Ki = 0;
    PID_ang.Kd = -0.02;
    PID_ang.OUT_MAX = 100;
    PID_ang.OUT_MIN = -100;
    PID_ang.err = 0;
    #ifdef PID_DELTA
    PID_ang.err_Pre = 0;
    PID_ang.err_PPre = 0;
    #endif
    PID_ang.target = 0;
    
    // 速度环
    //#define DEBUGING_ANG_PID
    
    #ifndef DEBUGING_ANG_PID
    PID_vel.Kp = -0.05;
    PID_vel.Ki = -0.0001;
    PID_vel.Kd = 0;
    #else
    PID_vel.Kp = 0;
    PID_vel.Ki = 0;
    PID_vel.Kd = 0;
    #endif
    PID_vel.OUT_MAX = 70;       // 经过测试，角度环最多不超过50
    PID_vel.OUT_MIN = -70;
    PID_vel.err =0;
    #ifdef PID_DELTA
    PID_vel.err_PPre = 0;
    #endif
    PID_vel.err_Pre = 0;
    PID_vel.target = 0;
    PID_vel.SumI = 0;
    
    #ifdef PID_LIMIT_SEPARAT
    PID_vel.I_MAX = 35; // 设置积分最大值
    
    PID_ang.P_MAX = 50;
    PID_ang.D_MAX = 50;
    #endif
    
    // 旋转环
    PID_rotate.Kp = 0.05;
    PID_rotate.Ki = 0.001;
    PID_rotate.Kd = 0;
    PID_rotate.err = 0;
    PID_rotate.err_Pre = 0;
    PID_rotate.OUT_MAX = 40;
    PID_rotate.OUT_MIN = -40;
    PID_rotate.output = 0;
    PID_rotate.SumI = 0;
    PID_rotate.target = 0;
    #ifdef PID_LIMIT_SEPARAT
    PID_rotate.I_MAX = 30;
    #endif
    
    IMU = imuPtr;
    /* 静态线程初始化 */
    rt_thread_init(&thread_balance,
                    "balance thread",
                    thread_balance_Entry,
                    RT_NULL,
                    &stack_balance[0],
                    sizeof(stack_balance),
                    1,10);
    
    rt_thread_startup(&thread_balance);  //启动线程
    rt_kprintf("balance-thread start success!\n");
                    
}

static double PID_calculate(PID* _PIDer,double input)
{
    _PIDer->err = _PIDer->target - input;     // 误差计算
 
#ifdef PID_DELTA    
    // 增量PID
    _PIDer->output += _PIDer->Kp * ( _PIDer->err - _PIDer->err_Pre)   
                + _PIDer->Ki * _PIDer->err
                + _PIDer->Kd * ( _PIDer->err - 2*_PIDer->err_Pre + _PIDer->err_PPre);
    
    // 限幅
    if(_PIDer->output > _PIDer->OUT_MAX)
        _PIDer->output = _PIDer->OUT_MAX;
    else if(_PIDer->output < _PIDer->OUT_MIN)
        _PIDer->output = _PIDer->OUT_MIN;
    
    // 数据迭代
    _PIDer->err_PPre = _PIDer->err_Pre;
    _PIDer->err_Pre = _PIDer->err;
    
    // 输出
    return _PIDer->output;

#else 
    // 状态式PID
    _PIDer->SumI += _PIDer->Ki * _PIDer->err;    // 积分项累加
    if(_PIDer->SumI > _PIDer->OUT_MAX)  _PIDer->SumI = _PIDer->OUT_MAX;     // 积分限幅
    else if(_PIDer->SumI < _PIDer->OUT_MIN) _PIDer->SumI = _PIDer->OUT_MIN;
    
    _PIDer->output = _PIDer->Kp * _PIDer->err +
                    _PIDer->SumI +
                    _PIDer->Kd * (_PIDer->err - _PIDer->err_Pre);
                
    if(_PIDer->output > _PIDer->OUT_MAX)    _PIDer->output = _PIDer->OUT_MAX;   // 输出限幅
    else if(_PIDer->output < _PIDer->OUT_MIN)   _PIDer->output = _PIDer->OUT_MIN;
    
    _PIDer->err_Pre = _PIDer->err;  //误差记忆迭代
    
    return _PIDer->output;
#endif
}

#ifdef DISTANCE_CTRL
static double PI_calculate(PID* _PIDer,double input,double integralComp)
{
    _PIDer->err = _PIDer->target - input;     // 误差计算
 
    // 状态式PID
    _PIDer->SumI += _PIDer->Ki * _PIDer->err;    // 积分项累加
    
    _PIDer->SumI -= integralComp;   // 积分补偿量
    
    if(_PIDer->SumI > _PIDer->I_MAX)  _PIDer->SumI = _PIDer->I_MAX;     // 积分限幅
    else if(_PIDer->SumI < -_PIDer->I_MAX) _PIDer->SumI = -_PIDer->I_MAX;
    
    _PIDer->output = _PIDer->Kp * _PIDer->err +
                    _PIDer->SumI;
                
    if(_PIDer->output > _PIDer->OUT_MAX)    _PIDer->output = _PIDer->OUT_MAX;   // 输出限幅
    else if(_PIDer->output < _PIDer->OUT_MIN)   _PIDer->output = _PIDer->OUT_MIN;
    
    return _PIDer->output;
}

#endif


#ifdef NONE_MOT_LOOP
static double PD_calculate(PID* _PIDer,double input,double inputD)
{
    _PIDer->err = _PIDer->target - input;     // 误差计算
    
    // 普通PD计算
    #ifndef PID_LIMIT_SEPARAT
    _PIDer->output = _PIDer->Kp * _PIDer->err
                + _PIDer->Kd * inputD;
    #else
    // 各单项限幅
    double cache = _PIDer->Kp * _PIDer->err;
    if(cache > _PIDer->P_MAX)           cache = _PIDer->P_MAX;
    else if(cache < (-_PIDer->P_MAX))   cache = - _PIDer->P_MAX;
    _PIDer->output = cache;
    
    cache = _PIDer->Kd * inputD;
    if(cache > _PIDer->D_MAX)           cache = _PIDer->D_MAX;
    else if(cache < (-_PIDer->D_MAX))   cache = - _PIDer->D_MAX;
    _PIDer->output += cache;
    
    #endif
    
    // 限幅
    if(_PIDer->output > _PIDer->OUT_MAX)
        _PIDer->output = _PIDer->OUT_MAX;
    else if(_PIDer->output < _PIDer->OUT_MIN)
        _PIDer->output = _PIDer->OUT_MIN;
    
//    // 数据迭代
//    _PIDer->err_PPre = _PIDer->err_Pre;
//    _PIDer->err_Pre = _PIDer->err;
    
    // 输出
    return _PIDer->output;
}
#endif


/*-----------------------------以下为用户FinSH接口--------------------------------------------*/
#ifdef DISTANCE_CTRL

/** @brief  设置小车前进后退的距离
 *  @note   调试使用，设置为PID的目标量
 *  @author 江榕煜（2021.6.12）
 *  @param  distance(double)
 *  @retval None
**/
void velThres(int argn,char ** argv)
{
//    ANGLE_threshold_VEL*ANGLE_threshold_VEL_FACTOR
    switch(argn)
    {
        case(1):
            rt_kprintf("速度阈值为:(%d,%d)\n",ANGLE_threshold_VEL,ANGLE_threshold_VEL_FACTOR);
            return;
        case(3):
            sscanf(argv[2],"%d",&ANGLE_threshold_VEL_FACTOR);
        case(2):
            sscanf(argv[1],"%d",&ANGLE_threshold_VEL);
            rt_kprintf("设置新阈值参数成功\n");
            return;
            
    }
}
MSH_CMD_EXPORT(velThres,设置速度阈值);


/** @brief  设置小车前进后退的距离
 *  @note   调试使用，设置为PID的目标量
 *  @author 江榕煜（2021.6.12）
 *  @param  distance(double)
 *  @retval None
**/
void setDistance(int argn,char ** argv)
{
    
    if(argn < 2)
    {
        rt_kprintf("参数不够!\n");
        return;
    }
    
    sscanf(argv[1],"%lf",&distance);
    
    PID_vel.target = 0; // 前进情况下，控速应该还是为0
    
    rt_kprintf("成功设置距离");
}
MSH_CMD_EXPORT(setDistance,设置小车移动距离);

#endif

/** @brief  设置小车前进后退的速度
 *  @note   调试使用，设置为PID的目标量
 *  @author 江榕煜（2021.6.12）
 *  @param  speed(int)
 *  @retval None
**/
void setSpeed(int argn,char ** argv)
{
    
    if(argn < 2)
    {
        rt_kprintf("参数不够!\n");
        return;
    }
    
    #ifdef VEL_CTRL_V1
    int speed;
    sscanf(argv[1],"%d",&speed);
    
    PID_vel.target = speed;
    PID_vel.SumI = 0;   // 积分项清零
    #endif
    #ifdef VEL_CTRL_V2
    
    sscanf(argv[1],"%lf",&vel_DeltaI);
    
    #endif
    
    rt_kprintf("成功设置速度\n");
}
MSH_CMD_EXPORT(setSpeed,设置小车速度);

/** @brief  设置小车旋转的速度
 *  @note   调试使用，设置为PID的目标量
 *  @author 江榕煜（2021.6.12）
 *  @param  speed(int)
 *  @retval None
**/
void setRot(int argn,char ** argv)
{
    
    if(argn < 2)
    {
        rt_kprintf("参数不够!\n");
        return;
    }
    
    int speed;
    sscanf(argv[1],"%d",&speed);
    
    PID_rotate.target = speed;
    PID_rotate.SumI = 0;   // 积分项清零
    
    rt_kprintf("成功设置速度\n");
}
MSH_CMD_EXPORT(setRot,设置小车旋转速度);


/** @brief  调整pid参数
 *  @note   两个控制环都在这个接口进行调试
 *  @author 江榕煜（2021.5.31）
 *  @param  
 *      ctrlLoop(char[])    控制环选择
 *      PIDparam(char[])    PID环
 *      value(double)        控制量
 *  @retval
 *      None
**/
int setPID(int argn,char ** argv)
{
    if(argn < 4)
    {
        rt_kprintf("参数不够!\n");
        return -1;
    }
    
    float value;
    sscanf(argv[3],"%f",&value);
    // 数据解析
    
    switch(argv[1][0])
    {
        case('v'):  // 速度环
            switch(argv[2][0])
            {
                case('p'):
                    PID_vel.Kp = value;
                    rt_kprintf("设置速度环P参数为:");
                    rt_kprintf(argv[3]); rt_kprintf("\n");
                    return 0;
                case('i'):
                    PID_vel.Ki = value;
                    PID_vel.SumI = 0;
                    PID_vel.output = 0;
                    rt_kprintf("设置速度环I参数为:");
                    rt_kprintf(argv[3]); rt_kprintf("\n");
                    return 0;
                case('r'):  // 输出范围设置
                    PID_vel.OUT_MAX = value;
                    PID_vel.OUT_MIN = -value;
                    rt_kprintf("设置速度环范围成功 \n");
                    return 0;
                case('I'):  // 设置积分输出最大值
                    PID_vel.I_MAX = value;
                    rt_kprintf("设置积分饱和值成功 \n");
                    return 0;
                default:
                    rt_kprintf("无效参数2 \n");
            }
            break;
        case('a'):  // 角度环
            switch(argv[2][0])
            {
                case('p'):
                    PID_ang.Kp = value;
                    rt_kprintf("设置角度环P参数为:");
                    rt_kprintf(argv[3]); rt_kprintf("\n");
                    return 0;
                case('d'):
                    PID_ang.Kd = value;
                    rt_kprintf("设置角度环D参数为:");
                    rt_kprintf(argv[3]); rt_kprintf("\n");
                    return 0;
                default:
                    rt_kprintf("无效参数2\n");
            }
            break;
        case('t'):  // 转向环
            switch(argv[2][0])
            {
                case('p'):
                    PID_rotate.Kp = value;
                    rt_kprintf("设置旋转环P参数为:");
                    rt_kprintf(argv[3]); rt_kprintf("\n");
                    return 0;
                case('i'):
                    PID_rotate.Ki = value;
                    rt_kprintf("设置旋转环I参数为:");
                    rt_kprintf(argv[3]); rt_kprintf("\n");
                    return 0;
                default:
                    rt_kprintf("无效参数2\n");
            }
            break;
        default:
            rt_kprintf("无效参数1\n");
            break;
    }
    return -1;
}
MSH_CMD_EXPORT(setPID, set PID params);

/** @brief  显示PID参数
 *  @note
 *  @author 江榕煜（2021.6.12）
 *  @param 
 *  @retval
**/
static void showPID(void)
{
    static char string[60];
    sprintf(string,"角度环P:%8.6f\n",PID_ang.Kp);
    rt_kprintf(string);
    sprintf(string,"角度环D:%8.6f\n",PID_ang.Kd);
    rt_kprintf(string);
    
    sprintf(string,"速度环P:%8.6f\n",PID_vel.Kp);
    rt_kprintf(string);
    sprintf(string,"速度环I:%8.6f\n",PID_vel.Ki);
    rt_kprintf(string);
    sprintf(string,"速度环输出范围:%8.6f 至 %8.6f\n",PID_vel.OUT_MIN,PID_vel.OUT_MAX);
    rt_kprintf(string);
    sprintf(string,"速度环积分饱和:%8.4f\n",PID_vel.I_MAX);
    rt_kprintf(string);
    
    
    sprintf(string,"转向环P:%8.6f\n",PID_rotate.Kp);
    rt_kprintf(string);
    sprintf(string,"转向环I:%8.6f\n",PID_rotate.Ki);
    rt_kprintf(string);
    
    /* 打印其他参数 */
    sprintf(string,"速度:%12.10f\n",vel_DeltaI);
    rt_kprintf(string);
    sprintf(string,"旋转速度:%8.6f\n",PID_rotate.target);
    rt_kprintf(string);
    
}
MSH_CMD_EXPORT(showPID,show PID param);

/** @brief  设置IMU机械安装偏移量
 *  @note   X轴
 *  @author 江榕煜（2021.6.12）
 *  @param  
 *  @retval
**/
static void setAngBias(int argn,char **argv)
{
    if(argn <2)
    {
        rt_kprintf("参数数量错误\n参数1为最小值，参数2为最大值\n");
        return;
    }
    
    float value;
    sscanf(argv[1],"%f",&value);
    angle_bias = value;
    rt_kprintf("设置新的偏移量成功 ");
}

MSH_CMD_EXPORT(setAngBias,设置机械中值);


/** @brief  关停平衡控制
 *  @note   加参数n可使能控制
 *  @author 江榕煜（2021.6.6）
 *  @param  
 *  @retval
**/
int disBalance(int argn,char ** argv)
{
    if(argn == 2 && (argv[1][0] == 'n'))
    {
//        PID_vel.SumI = 0;
//        PID_vel.output  =0;
//        
//        TRIG_BALANCE_ENABLE = 1;
        rt_kprintf("使能平衡控制\n");
        ENABLE_BALANCE_CTRL();
        return 0;
    }
    
    rt_kprintf("关停平衡控制\n");
//    
//    TRIG_BALANCE_ENABLE = 0;
//    motorDuty_Left = 0; // 关停电机
//    motorDuty_Right = 0;
//    PID_vel.SumI = 0;   PID_vel.output = 0;
//    PID_ang.output = 0;
//    
//    /* 设置电机转速 */
//    DCMotor_SetSpeed(0,0);
//    DCMotor_SetSpeed(1,0);
    
    DISABLE_BALANCE_CTRL();
    return 0;
}

MSH_CMD_EXPORT(disBalance, disable the balance control);


